#include "Control.h"

float Med_Angle = -1;                             // 机械中值
float Vertical_Kp = 0.001, Vertical_Ki = 0.002;          // 直立环
float Velocity_Kp = 0.5, Velocity_Ki = 0.255, a = 0.7; // 速度环

int16_t Vertical_Out, Velocity_Out, Turn_Out;

void EXTI9_5_IRQHandler(void)
{
    if (EXTI_GetFlagStatus(EXTI_Line5) != 0)
    {
        int16_t PWM_Out;
        if (GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5) == 0)
        {
            EXTI_ClearITPendingBit(EXTI_Line5);

            // 采集编码器数据与MPU6050角度信息
            Encoder_left = Read_Speed(2);
            Encoder_right = Read_Speed(4);
					  OLED_ShowSignedNum(1, 1, Encoder_left, 5);
					  OLED_ShowSignedNum(4, 1, Encoder_right, 5);
            mpu_dmp_get_data(&Pitch, &Roll, &Yaw);
            MPU_Get_Gyroscope(&GX, &GY, &GZ);
            MPU_Get_Accelerometer(&AX, &AY, &AZ);
            // 将数据放入闭环控制器中，计算出控制输出量
            Vertical_Out = Vertical(Med_Angle, Pitch, GY);
            Velocity_Out = Velocity(Encoder_left, Encoder_right);
            Turn_Out = Turn(GZ);

            PWM_Out = Vertical_Out - Vertical_Kp * Velocity_Out;
            // 把控制量加载到电机上
            MOTOR1 = PWM_Out - Turn_Out;
            MOTOR2 = PWM_Out + Turn_Out;
            Limit(&MOTOR1, &MOTOR2);
            Load(MOTOR1, MOTOR2);
        }
        EXTI_ClearFlag(EXTI_Line5);
    }
}

/****************
 直立环：Kp*Ek+Kd*Ek_D
 入口：期望角度，真实角度，真实角速度
*****************/
int16_t Vertical(float Med, float Angle, float gyro_Y)
{
    int16_t PWM_Out;

    PWM_Out = Vertical_Kp * (Angle - Med) + Vertical_Ki * (gyro_Y - 0);
    return PWM_Out;
}

/*******************
 速度环：Kp*Ek+Ki*Ek_S
*******************/
int16_t Velocity(int16_t encoder_left, int16_t encoder_right)
{
    int16_t PWM_Out;
    static int16_t Encoder_Err, Encoder_S, EnC_Err_LowOut, EnC_Err_LowOut_Last;

    Encoder_Err = encoder_left + encoder_right - 0;

    EnC_Err_LowOut = (1 - a) * Encoder_Err + a * EnC_Err_LowOut_Last;
    EnC_Err_LowOut_Last = EnC_Err_LowOut;

    Encoder_S += EnC_Err_LowOut;
    Encoder_S = Encoder_S > 10000 ? 10000 : (Encoder_S < -10000 ? -10000 : Encoder_S);
    PWM_Out = Velocity_Kp * EnC_Err_LowOut + Velocity_Ki * Encoder_S;

    return PWM_Out;
}

/*****************
转向环：系数*Z轴角速度
******************/
int16_t Turn(int gyro_Z)
{
    int16_t PWM_Out;

    PWM_Out = gyro_Z * (0.5);
    return PWM_Out;
}
